/*
* 时间：2021.07.22
* 代码来源：公众号【从0到1的点云】
* 功能：实现点云的边缘提取
*/
#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/boundary.h>
#include <math.h>
#include <boost/make_shared.hpp>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/covariance_sampling.h>
#include <pcl/filters/normal_space.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <pcl\point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/kdtree/kdtree_flann.h>
//法向量估计(支持openmp并行)
pcl::PointCloud<pcl::Normal>::Ptr my_normal(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
  n.setInputCloud(cloud);
  n.setNumberOfThreads(8);//设置openMP的线程数
  n.setSearchMethod(tree);// 输入搜索方法
  n.setKSearch(10);//n.setRadiusSearch(num);半径搜索
  n.compute(*normals);
  return normals;
}

pcl::PointCloud<pcl::Boundary> my_estimateBorders(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud)
{

  pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> boundEst; //定义一个进行边界特征估计的对象
  pcl::PointCloud<pcl::Normal>::Ptr normals= my_normal(cloud); //保存法线估计的结果
  boundEst.setInputCloud(cloud); //设置输入的点云
  boundEst.setInputNormals(normals); //设置边界估计的法线，因为边界估计依赖于法线
  boundEst.setRadiusSearch(1.0); //设置边界估计所需要的半径,//这里的Threadshold为一个浮点值，可取点云模型密度的10倍
  boundEst.setAngleThreshold(M_PI / 4); //边界估计时的角度阈值M_PI / 4  并计算k邻域点的法线夹角,若大于阈值则为边界特征点
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  boundEst.setSearchMethod(tree); //设置搜索方式KdTree
  pcl::PointCloud<pcl::Boundary> boundary; //保存边界估计结果
  boundEst.compute(boundary); //将边界估计结果保存在boundaries 仅仅是点的索引
  std::cout << "boundary计算结束...\n";
  return boundary;
}
int
main(int argc, char** argv)
{
  srand(time(NULL));
  float re = 1.0, reforn = 20.0;
  /*re = std::atof(argv[2]);
  reforn = std::atof(argv[3]);*/
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile("1.pcd", *cloud); 
  //创建滤波器对象
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud(cloud);
  sor.setMeanK(100);//寻找每个点的50个最近邻点
  sor.setStddevMulThresh(3.0);//一个点的最近邻距离超过全局平均距离的一个标准差以上，就会舍弃
  sor.filter(*cloud_filtered);
  //std::cout << "cloud_src: " << cloud_src->points.size() << std::endl;
  //std::cout << "cloud_filtered: " << cloud_filtered->points.size() << std::endl;

  pcl::PointCloud<pcl::Boundary> boundary=my_estimateBorders(cloud);

  //转为pointxyz类型
  pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
  for (int i = 0; i < boundary.size(); i++)
  {
    if (boundary[i].boundary_point > 0)
    {
      target->push_back(cloud->points[i]);
     }
  }

  std::cout << "正在创建窗口...\n";
  pcl::visualization::PCLVisualizer viewer("boundary extract");
  // 第一个窗口
  int v1(0);
  viewer.createViewPort(0, 0, 0.5, 1, v1);//Xmin,Ymin,Xmax,Ymax,view_ID
  viewer.setBackgroundColor(0, 0, 0, v1);//R,G,B,view_ID
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud, 255, 0, 0);
  viewer.addPointCloud<pcl::PointXYZ>(cloud,single_color1,"cloud",v1);
  // 第二个窗口
  int v2(1);
  viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
  viewer.setBackgroundColor(0, 0, 0, v2);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(target, 0, 255, 0);
  viewer.addPointCloud<pcl::PointXYZ>(target, single_color2, "boundary_cloud", v2);

  std::cout << "结束...";
  while (!viewer.wasStopped())
  {
    viewer.spinOnce(100);//显示每次调用spinonce，响应键盘鼠标事件，有一种spin重载可以只调用一次
    boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }
  return 0;
}